#ifndef MATCH_CENTER_H
#define MATCH_CENTER_H

#include <Eigen/Dense>
#include <iostream>
#include <vector>
using Eigen::MatrixXd;
using Eigen::VectorXd;
#include "common/algorithm_type/all_type.h"
// #include "common/log.h"
#include "kalman/kalman.h"
#include "match.h"
namespace perception
{
    namespace algorithm
    {
        using namespace Eigen;
        class MatchCenter
        {
        public:
            MatchCenter(track_config &config);
            unsigned long int id;  // identifier for the cluster
            unsigned long int age; // age of the cluster

            void Init(unsigned long int id, LidarObject &new_points, const double &time, const Localization &ego_pose);
            void update(LidarObject &new_points, const double &time, const Localization &ego_pose);
            void predict();
            void update(const double &time, const Localization &ego_pose);
            LshapeTracker Lshape;

            double confidence;
            double max_confidence = 10;
            double dconfidence = 1;

            double old_thetaL1;
            double L1, L2, thetaL1, thetaL2;
            double cx, cy, cvx, cvy, L1_box, L2_box, th, psi, comega, length_box, width_box, height_box; // psi=heading
            double ax, ay;
            double x_ukf, y_ukf, vx_ukf, vy_ukf, omega_ukf;

            double x;
            double y;
            double z;
            double last_time;
            CornerPoint cp;
            // double
            int type;
            LidarObject pre_obj;
            // std::vector<center>
            // mean value of the cluster

            int track_type_ = track_type::NEW_OBJ;

            std::vector<CornerPoint> all_points; // 这里是目标的中心点和合集0
            float sum_x = 0;
            float sum_y = 0;
            float pre_heading;
            KalmanFilterSimple dynamic_kf;
            int points_num = 0;

            bool GetObj(LidarObject &new_points);

        private:
            float now_v_ratio = 0.8;
            float max_a = 10;
            float dheading = 10 * deg2rad;
            bool CheckStatic();
            // 估计shape及中心点
            bool EsShape(LidarObject &new_points);
            void CoordinateTransformation(std::vector<CornerPoint> &cells,
                                          const Localization &loc)
            {
                auto tmp = cells;
                for (size_t i = 0; i < cells.size(); i++)
                {
                    cells[i].x = (tmp[i].x - loc.xg) * std::cos(loc.angle * deg2rad) +
                                 (tmp[i].y - loc.yg) * std::sin(loc.angle * deg2rad);
                    cells[i].y = (tmp[i].y - loc.yg) * std::cos(loc.angle * deg2rad) -
                                 (tmp[i].x - loc.xg) * std::sin(loc.angle * deg2rad);
                }
            }

            // 构建设计矩阵 X 和观测向量 y
            void buildSystem(const std::vector<CornerPoint> &points,
                             int degree, MatrixXd &X, VectorXd &y_vec)
            {
                int n = points.size();
                X.resize(n, degree + 1); // 多项式的最高次数加1
                y_vec.resize(n);

                for (int i = 0; i < n; ++i)
                {
                    X(i, 0) = 1.0; // 常数项
                    for (int j = 1; j <= degree; ++j)
                    {
                        X(i, j) = std::pow(points[i].x, j);
                    }
                    y_vec(i) = points[i].y;
                }
            }
        };
    }
}
#endif